#!/usr/bin/env python3

# Software License Agreement (BSD License)
# Copyright © 2019, 2022-2023 belongs to Shadow Robot Company Ltd.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without modification,
# are permitted provided that the following conditions are met:
#   1. Redistributions of source code must retain the above copyright notice,
#      this list of conditions and the following disclaimer.
#   2. Redistributions in binary form must reproduce the above copyright notice,
#      this list of conditions and the following disclaimer in the documentation
#      and/or other materials provided with the distribution.
#   3. Neither the name of Shadow Robot Company Ltd nor the names of its contributors
#      may be used to endorse or promote products derived from this software without
#      specific prior written permission.
#
# This software is provided by Shadow Robot Company Ltd "as is" and any express
# or implied warranties, including, but not limited to, the implied warranties of
# merchantability and fitness for a particular purpose are disclaimed. In no event
# shall the copyright holder be liable for any direct, indirect, incidental, special,
# exemplary, or consequential damages (including, but not limited to, procurement of
# substitute goods or services; loss of use, data, or profits; or business interruption)
# however caused and on any theory of liability, whether in contract, strict liability,
# or tort (including negligence or otherwise) arising in any way out of the use of this
# software, even if advised of the possibility of such damage.


import sys
import rospy
from sr_robot_commander.sr_robot_state_combiner import SrRobotStateCombiner


# This script provides a command line interface to combine arm joints from one robot_state with hand joints from
# another robot_state. States are identified using their name in the DB
# To use this script:
# rosrun sr_robot_commander combine_robot_state ARM_STATE_NAME HAND_STATE_NAME NAME_TO_SAVE
# NAME_TO_SAVE is the name that will be used for the state in the database.
# N.B. IF A STATE ALREADY EXISTS WITH THAT NAME IT WILL BE OVERWRITTEN WITH NO WARNING!
# ARM_STATE_NAME specifies the name of the state from which the arm joints have to be read
# HAND_STATE_NAME specifies the name of the state from which the hand joints have to be read
# If you don't want to use one of the two arm or hand, you can replace .


if __name__ == "__main__":
    rospy.init_node("state_saver")
    if len(sys.argv) <= 3 or sys.argv[1] == '' or sys.argv[2] == '' or sys.argv[3] == '':
        rospy.logerr("You didn't enter names for ARM_STATE_NAME HAND_STATE_NAME and NAME_TO_SAVE.")
        sys.exit(-1)

    if len(sys.argv) > 3:
        arm_state = sys.argv[1]
        hand_state = sys.argv[2]
        new_state = sys.argv[3]
        combiner = SrRobotStateCombiner(arm_state, hand_state, new_state)
        combiner.combine()
